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Abstract: To estimate the smoothing distribution in a nonlinear state space model, we apply 
the conditional particle filter with ancestor sampling. This gives an iterative algorithm in a 
Markov chain Monte Carlo fashion, with asymptotic convergence results. The computational 
complexity is analyzed, and our proposed algorithm is successfully applied to the challenging 
problem of sensor fusion between ultrawideband and accelerometer/gyroscope measurements for 
indoor positioning. It appears to be a competitive alternative to existing nonlinear smoothing 
algorithms, in particular the forward filtering-backward simulation smoother. 


1. INTRODUCTION 

Consider the (time-varying, nonlinear, non-Gaussian) state 
space model (SSM) 

Xt+i I Xt ~ ft{xt+i\xt), (la) 

yt\xt-^ gtiyt\xt), (lb) 

with xi ~ ti(xi). We use a probabilistic notation, with 
~ meaning distributed according to. The index variable 
t = 1,2,..., T is referred to as time. The variable Xt G M””" 
is referred to as state, and an exogenous input Ut is possible 
to include in ft and gt. To ease the notation, the possible 
time dependence of / and g will be suppressed. 

For some applications, e.g., system identification, the dis¬ 
tribution of the states for given model and measurements, 

p{xi.,T\yi-.T), ( 2 ) 

is of interest. We will refer to (2) as the smoothing distribu¬ 
tion. The smoothing distribution is not available on closed 
form for the general model (1), and approximations are 
necessary. In this paper, we will present a method gener¬ 
ating Monte Carlo samples, particles, from the smoothing 
distribution, akin to a particle filter. The idea is to iterate 
a conditional particle filter, which generates samples from 
the smoothing distribution after sufficiently many itera¬ 
tions, as illustrated in Figure I. 

An overview of existing particle smoothers addressing 
the problem of generating samples from (2) is provided 
by Lindsten and Schon (2013). In this work we will in 
particular compare and relate our developments to the 
so-called forward filtering-backward simulation (FFBSi) 
smoother introduced by Douc et al. (2011). The work 
by Kitagawa (1996) and Briers et al. (2010) are both of 
interest in that they approach a similar problem using 
particle filters, the latter also taking inspiration from the 
two-filter formula. The work of Pillonetti and Bell (2008) 
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• Particles in conditional particle filter 

- Conditional trajectory 

. - Tfue smoothing distribution 

Fig. 1. How to use the conditional particle filter to sample 
from the smoothing distribution Th® distribution 

p(2:i:T|yi:T) is shown in gray, with time t at the horizontal 
axis and state x on the vertical axis. The conditional particle 
filter (with only 2 particles) is run iteratively, starting with the 
its arbitrary initialized trajectory (blue line) in the top plot, 
eventually converging to provide samples from the smoothing 
distribution. 










is closely related in that they also employ a Markov chain 
Monte Carlo (MCMC) construction, but they consider 
the special case of Gaussian noise (/ and g can still be 
nonlinear though). 

An alternative to using samples to represent (2) is to 
solve a linearized version of the problem, by combining the 
extended Kalman filter (Smith et ah, 1962; Schmidt, 1966) 
with the RTS-smoother (Rauch et ah, 1965) to solve the 
linearized problem analytically. The work of Sarkka (2008) 
on the unscented RTS-smoother is along the same line. 

It should also be possible to generalise the ideas presented 
in this paper to probabilistic graphical models along the 
lines of the work by Naesseth et al. (2014). 

Source code for simulated examples are available via 
the first author’s homepage, and details on the second 
simulated example and the indoor positioning problem are 
available in a technical report (Svensson et ah, 2015). 

2. PARTICLE METHODS 


Algorithm 1 Particle Filter 

Output: A weighted particle system raJjA ^ 
approximating p{xi-t\yi-.t) for t = 1, 2,..., T. 

1: Draw x\ ~ qi{xi) for i = 1,... A. 

2: Set w\ = Wi(x\) for i = 1,... A. 

3: for t = 2,... ,T do 

4: Draw al with P (a) = j) oc for i = 1,... A. 

5: Draw xl qt{xt\XfLj^,yt) for i = 1,... A. 

6: Set x\.f. = {xilf_i,xl} for i = 1,... A. 

7: Set wl = W{x\.j.) for z = 1,... A. 

8: end for 



5 10 15 20 25 30 35 40 


Time 


We assume the reader has some basic familiarity with 
particle filters (see, e.g., Doucet and Johansen (2011) for 
an introduction), but to set the notation we will start by 
a brief summary of particle filters and particle smoothers. 


2.1 Particle filters 


In the search for a numerical approximation top(a;i: 7 ’|?/i: 7 ’), 
the following factorization is useful 

T T-1 

p{xi,T,yi,T) = p{xi)W_g(jjt\xt) Yl fixt+i\xt), (3) 

t=i t=i 


as it allows for the following recursion to be derived (using 


Bayes’ rule and p(?/i:t) = p{yi)YlJ=i p{yt+i\yi-.t)) 

, I X g{yt\xt)f{xt\xt-i) , I X /.X 

p{xi-.t\yi-.t) — - j ^-;;-p(a;i:t-i|yi:t-i). (4) 

p[yt\yi:t-i) 


(4*) 


This factorization can be used to motivate the particle 
filter. Starting with a particle (Monte Carlo) approxima¬ 
tion of p{xi\yi) as N particles, (4i*:) can be applied to 
obtain a particle approximation of p{xi: 2 \yi-. 2 )- Repeating 
this T — 1 times, a particle approximation of pixi-xlyi-.r) 
as N weighted particles {x\.rp,w^}fLi is found. This is 
detailed in Algorithm 1, the particle filter^ a Sequential 
Monte Carlo method. 


Fig. 2. Path degeneracy: The particles in a particle filter are 
shown as dots, propagated as the lines indicate. The red 
dots are particles that have ‘survived’ the resampling steps, 
whereas the grey dots have not ‘survived’ the resampling steps. 
All trajectories have the part xi-.is in common. This 

phenomenon occurs in particle filters and is the reason why a 
particle filter does not provide a good numerical approximation 
of p{xi:T\yi:T) for ^ finite N. 

the resampling step, to prepare for the expansion to 
conditional particle filter with ancestor sampling. 

In theory, a particle filter directly gives a numerical ap¬ 
proximation of p{xi-,T\yi-.T)- However, in practice with a 
finite A, the approximation tends to be rather poor (unless 
T is very small), as it typically suffers from path degeneracy 
as illustrated in Figure 2. 

2.2 Forward - backward particle smoothers 

A natural way to find the smoothing distribution for 
SSMs is to first apply a (forward) filter, and then add 
a backward pass, adjusting for the ‘new’ information 
about the state Xt at time t obtained from the later 
measurements yt+i, ■ ■ ■ ,yT- Such an example is the RTS 
smoother for the linear Gaussian case (Rauch et ah, 1965), 
but also the more recent particle-based FFBSi algorithm, 
see, e.g., Lindsten and Schdn (2013) for a recent overview. 

The algorithm for FFBSi is not repeated here, but we note 
that the it relies on the two step 


The notation used in Algorithm 1 is 

Wi{xi) = g{yi\xi)p,{xi)/qi{xi\yi) (5a) 

W{xi,t) = g{yt\xt)f{xt\xt-i)/q{xt\xt-i,yt). (5b) 

Here, q denotes the proposal distribution, which is used 
to propagate the particles from time t to time t J- 1. 
If the proposal q is chosen as /, then (5b) simplifies to 
W{xi:t) = g{yt\xt) resulting in the so-called bootstrap 
particle filter. 

The main steps in Algorithm I, namely 4, 5 and 7 are 
often referred to as resampling, propagation and weighting, 
respectively. Step 6 is merely bookkeeping. In Algorithm 1, 
a notation using ancestor indices a] has been used for 


(1) A particle filter with A particles. 

(2) A backward simulation drawing M (uncorrelated) 
samples from p{xi,T\yi-.T) using the A particles from 
Step (1). 

The computational complexity of FFBSi is basically 
0{NM), although some improvements can be achieved, 
see (Lindsten and Schon, 2013, Section 3.3). 

To prepare for the upcoming discussions on convergence, 
let us briefly comment on the convergence properties of 
FFBSi. 

How well can a function h(xi,T) be approximated as h 
using samples from FFBSi? Let h^^BSi = F Tti 















denote an approximation of h{xi-,T) based on M = N 
backward trajectories. Under some fairly mild assump¬ 
tions, it has been shown (Done et ah, 2011, Corollary 9) 
that there exists a crpFBSi < cx) such that 

\/lV(/lFFBSi-E[^(a^l:T)|yi:T]) (6) 

converges weakly to A/’(0, app^g;). 

To summarize, the convergence rate for FFBSi is VN, 
subject to a computational complexity of 0{N‘^). 

3. SMOOTHING USING THE CONDITIONAL 
PARTICLE FILTER 

The smoothing methodology discussed in Section 2.2 
builds on a forward-backward strategy. The MCMC 
idea offers a fundamentally different way to construct a 
smoother, without explicitly running a backward pass, but 
iteratively running a so-called conditional particle filter as 
illustrated in Figure 1. As we will see, this opens up for 
a reduced computational complexity. The origin of the 
method dates back to the introduction of the PMCMC 
methods by Andrieu et al. (2010), with important recent 
contributions from Lindsten et al. (2014). 

First, the conditional particle filter with ancestor sampling 
(CPF-AS) will be introduced (Section 3.1), followed by a 
brief introduction to MCMC (Section 3.2), and they will in 
the next step (Section 3.3) be combined to form a particle 
smoother. The convergence properties and the computa¬ 
tional complexity of the smoother are then examined in 
Section 3.4 and Section 3.5, respectively. 

3.1 Conditional particle filter with ancestor sampling 

The CPF-AS is thoroughly described by Lindsten et al. 
(2014), and here presented as Algorithm 2. The CPF-AS 
is similar to a regular particle filter. Algorithm 1, in many 
aspects, but with one particle trajectory xi-,T[k] specified 
a priori (trajectory number N in Algorithm 2). 

The CPF-AS generates N weighted particle trajectories 
{x\.rp,wlp}fLi. With the original formulation of the con¬ 
ditional particle filter in Andrieu et al. (2010), one of 
these trajectories is predestined to be xnrlk]. Extending 
this with ancestor sampling, the CPF-AS is obtained and 
the resulting trajectories are still influenced 

by xi.,T[k], but in a somewhat more involved way, as the 
conditional trajectory may be ‘partly’ replaced by a new 
trajectory; see Algorithm 2 for details. 

By sampling one of the trajectories xi.T[k + 1] = x(.rp 
obtained from the CPF-AS with P(i = J) oc wf, the 
CPF-AS can be seen as a procedure to stochastically map 
Xi:T[k] onto another trajectory Xi:T[k + 1]. 

A Rao-Blackwellized formulation of the CPF-AS for mixed 
linear/nonlinear models is also possible, see Svensson et al. 
(2014) for details. 

3.2 Markov chain Monte Carlo 

MCMC offers a strategy for sampling from a complicated 
probability distribution tt on the space Z, using an itera¬ 
tive scheme. 


Algorithm 2 Conditional particle filter with ancestor 
sampling (CPF-AS) 

Input: Trajectory xi:T[k] 

Output: Trajectory xnxlk + 1] 

1: Draw xl ~ gi(xl) for i = 1,..., N — 1. 

2 : Set X^ = Xi[k\. 

3: Set w\ = IUi(a:)) for i = 1,..., A. 

4: for t = 2,... ,T do 

5: Draw al with P (a) = j) oc wf_i for z = 1,... ,N — 1. 

6: Draw xl ~ q{xt\x'l:!_i,yt) for z = 1,..., A — 1. 

7: Set xf^ = Xt[k]. 

8: Draw with P (a(^ = j) oc w{_if{x^\x{_i). 

9: Set x\.t = x)} for I = 1,..., A. 

10: Set wl = W(x\.^) for z = 1,..., A. 

11: end for 

12: Draw J with P (z = J) oc wlp and set xi.,T[k+l] = x(.rp. 

A Markov chain on 2 is a sequence of the random variables 
{C[l], C[2], C[3], ■ • ■ C[^] € Z. The chain is defined by a 

kernel /C, stochastically mapping one element C[fc] onto 
another element C[A: + 1]. That is, the distribution of the 
random variable C[fc] depends on the previous element as 

C[A: + l]~/C(-|C[fc])). 

If the kernel K, is ergodic with a unique stationary distribu¬ 
tion TT, the marginal distribution of the chain will approach 
TT in the limit. Let (’[0] be an arbitrary initial state with 
’’■(CP]) > Oj then by the ergodic theorem (Robert and 
Casella, 2004) 

^EMCW)^e.[MC)], (7) 

k=l 

as K —>■ oo for any function h : {K"}^ i—)■ M, with [•] 
denoting expectation w.r.t. C under the distribution tt. 

That is, for sufficient large k, the realization of {C[^]j C[k + 
1],... } is (possibly correlated) samples from tt. This 
summarizes the idea of the MCMC methodology; if tt is of 
interest, construct a kernel K, with stationary distribution 
TT and simulate a Markov chain to obtain samples of tt. 

Note that any finite realization of the chain {C[l], • ■ •, C[7f]} 
may be an arbitrarily bad approximation of tt. This typi¬ 
cally depends on the initialization C[0] and on how well the 
kernel K, manages to explore Z, referred to as the mixing. 

3.3 Smoothing using MCMC 

Take the general space Z as the more concrete space 
{]gnx}T jji-T lives). Note that CPF-AS in Algo¬ 
rithm 2 maps one element in onto another element 

in and can therefore be interpreted as an MCMC 

kernel. The unique stationary distribution for CPF-AS 
is _p(xi:t|2/i:t) (which is far from obvious, but shown by 
Lindsten et al. (2014)). Now, by constructing a Markov 
chain. Algorithm 3 is obtained, generating samples from 
the distribution p(xi:7’|yi:T) (he-, a smoother). 

An illustration of Algorithm 3 was provided already by 
Figure 1; The initial trajectory is obviously not a sample 
from TT = p(xi: 7 ’|yi:T)) and artifacts from the initializa¬ 
tions appear to be present also in iteration [1], [2], and 
possibly [3]. However, iterations [5], [6], [7] appear to be 






Algorithm 3 MCMC smoother 

Input: (Initial (arbitrary) state trajectory) 

Output: ■ ■ ■, xi-t[K] {K samples from the 

Markov chain) 

1; for k = 1,..., K do 

2; Run the CPF-AS (Algorithm 2) conditional on 
xi:T[k — 1] to obtain xi:T[k]. 

3: end for 

(correlated) samples from the distribution tt, which is what 
was sought. 

3-4 Convergence 

The convergence analysis of Algorithm 3 can, similar to 
the FFBSi in Section 2.2, be posed as the question of 
how well h{xi:T) can be approximated by hQpp_Ag = 
Sfc=i h{xi:T[k]), where xi-,T[k] comes from Algorithm 3. 
Before stating the theorem, let us make the following two 
rather technical assumptions 

Al. The proposal q is designed such that given any Xt-i 
with non-zero probability (given the measurements 
any Xt with non-zero probability (given yi-t) 
should be contained in the support of q. 

A2. There exists a constant k < oo such that ||IF||oo < «• 

Theorem 1. (Convergence for Algorithm 3). Under the as¬ 
sumptions Al and A2, for any number of particles A > 1, 
and for any bounded function h : i—>• M, there 

exists a (7/i < oo such that 

V K (hcPF-AS - E [h{xi:T)\yi:T]) (8) 

converges weakly to A/’(0, ct^). 

Proof. The CPF-AS is uniformly ergodic for A > 1, 
(Lindsten et al., 2014, Theorem 3). Therefore (Liang et ah, 
2010, Theorem 1.5.4) is applicable. 

Note 1. The convergence of Algorithm 3 to the smoothing 
distribution is by Theorem 1 not dependent of the number 
of particles A —)■ oo, but is only relying on the number of 
iterations A —)■ oo. 

3.5 Computational complexity 

The computational complexity of Algorithm 3 is of order 
0{KN), where A is the number of particles in the CPF- 
AS and K the number of iterations. However, in some pro¬ 
gramming languages, e.g., Matlab, vectorized implementa¬ 
tions are preferable. The sequential nature of Algorithm 3 
in k does not allow such a vectorized implementation, 
which is a clear drawback. On the other hand, K does 
not have to be specified a priori, but Algorithm 3 can be 
run repeatedly until satisfactory results are obtained, or a 
given computational time limit is violated. 

The short message here is: The convergence rate for 
Algorithm 3 is Vk, obtained at a computational cost of 
0{K) (for a fixed number of particles A). This can be 
compared to the convergence rate ^/N to the less beneficial 
cost of 0{N'^) for FFBSi. However, one should remember 
that the samples obtained from FFBSi are uncorrelated, 
which is typically not the case for Algorithm 3. 


4. SIMULATED EXAMPLES 
4-1 Scalar linear Gaussian SSM 

As a hrst example, consider the scalar linear Gaussian SSM 
xt+i = D.2xt+ut+wt, wt ~ ^(0,0.3), (9a) 

yt = xt+et, et~A(0,1), (9b) 

with E [xi] = 0 and E [xf] = 0.1. Implementing Algo¬ 
rithm 3 with A = 2 (with T = 80 and Ut being low- 
pass filtered white noise), the result shown in Figure 1 is 
obtained. As the system is linear and Gaussian, analytical 
expressions for pixi-rlyi-.r) can be found using the RTS 
smoother, shown in gray in Figure 1. 

4-2 Nonlinear, multi-modal example 

We will now turn to a more challenging problem, pin¬ 
pointing some interesting differences between the forward- 
backward smoother (FFBSi) and our MCMC-based 
smoother in Algorithm 3. We will start with a discussion 
using intuitive arguments, to motivate the example. 

The FFBSi smoother handles the path degeneracy prob¬ 
lem in the particle filter discussed in Section 2. However, 
the support for the backward simulation is still limited to 
the particles sampled by the particle hlter. As those parti¬ 
cles, for t < T, are sampled from the filtering distribution 
pixt\yi:t) (and not the smoothing distribution p{xt\yi:T), 
due to the factorization (4)), only few of the particles may 
be useful if the difference between the filtering and smooth¬ 
ing distribution is ‘large’. This might cause a problem for 
the FFBSi smoother, since there might exist cases where 
the particles do not explore the relevant part of the state 
space. An interesting question is now if Algorithm 3 can 
be expected to explore the relevant part of the state space 
better than the FFBSi smoother? 

One way to understand the effect of the conditional tra¬ 
jectory in CPF-AS is as follows: If a proposal distribution 
q f is used in a regular particle hlter (Step 5 in 
Algorithm 1), it is compensated for in the update of the 
weights. Step 7 and (5b), so that {x\.f,wl}fLi are still an 
approximation of p{xi.t\yi:t), even ii q f. 

The CPF-AS can be thought of as a regular particle 
hlter, but with a ‘proposal’ q{xt) that deterministically 
sets xf = Xt[k] (Step 7 of Algorithm 2) and ‘artihcially’ 
assigns an ancestor to it (Step 8). However, there is no 
compensation for this ‘proposal’ in Step 10. Therefore, the 
samples {x\.^,wl}fLl from the CPF-AS can be expected 
to be biased towards the conditional trajectory xi:T[fc]- 

On the other hand, we know from Lindsten et al. (2014) 
that the conditional trajectories in the limit fc —)■ oo 
are samples of p{xi-,T\yi-.T)- The bias towards xi-rlk] in 
the CPF-AS can therefore be thought of as ‘forcing’ the 
CPF-AS to explore areas of the state space relevant for 
the smoothing distribution p{xi-T\yi-.T) (rather than the 
filtering distribution p(xi:t|?/i:t)) for large k. 

A simulated example, appealing to this discussion, is now 
given. The problem is to sample from the smoothing distri¬ 
bution for a one-dimensional SSM with multi-modal prop¬ 
erties of g{xt\yt). The state space model is /(xt+i|xt) = 
N{xt+i\xt,<j‘^) and g(yt\xt) is implicitly defined through 



-Filtering, N = 50 000 



Fig. 3. The ‘landscape’ for the Example in 4.2. The surface is 
proportional to g{yt\xt), and the axis are time t and state x, 
respectively. The trajectories are the mean of the results of 
a particle filter (Algorithm 1, filtering), FFBSi (smoothing) 
and Algorithm 3 (smoothing). Note that for both smoothing 
methods, a total of 50 000 particles were sampled, so the 
comparison is ‘fair’ in that sense. 

the surface in Figure 3, where the surface level in point 
{x,t) defines g{yt\xt), for a given yt (not shown). 

Given xq, finding the maximum a posteriori estimate of the 
smoothing distribution p(a:i;T|2/i:T) amounts to finding the 
path xi-,T maximizing p(a;i:7’I J/Iit) oc 

f{xt\xt-i)Ylf^ig{yt\xt), where g{yt\xt) is defined 
through the surface in Figure 3. Intuitively, this can be 
thought of as going from left (t = 0) to right {t = 100) in 
Figure 3, playing the children’s game ‘the floor is hot lava’ 
with the cost / for moving sideways. 

The mean of the filtering distribution p{xt\yi:t) for t = 
1,... ,T (obtained by Algorithm 1) is shown in Figure 3, 
together with the mean from two different smoothers; 
FFBSi (Section 2.2) and Algorithm 3, respectively. 

The two smoothing approximations can indeed be ex¬ 
pected to approach each other in the limit V —)■ oo / 
K ^ oo. The problem is interesting because the ‘likelihood 
landscape’ in Figure 3 contains a ‘trap’. The filtering 
distribution (and hence the particle filter in FFBSi) will 
follow the right ‘shoulder’ and discover ‘too late’ (the 
valley at t « 70) that it ‘should’ have walked along the left. 
The smoothing distribution, however, walks along the left 
shoulder earlier, as it ‘knows’ that the valley at the right 
hand side will come. 

To quantify this discussion on how well the particles 
explore the state space for the two smoothers, the densities 
of the sampled particles for both smoothers are plotted in 
Figure 4. This suggest that Algorithm 3 is able to give a 
better approximation of the smoothing distribution, as a 
larger proportion of the particles are sampled in a relevant 
part of the state space. 

5. INDOOR POSITIONING APPLICATION 

In this section, the presented algorithm is applied to 
a real-world sensor fusion problem; indoor positioning 
using ultrawideband (UWB), gyroscope and accelerometer 
measurements. We apply the model from Kok et al. (2015), 
but rather than using the optimization-based approach in 
that paper, we employ Algorithm 3. Instead of obtaining 
the Maximum a Posteriori (MAP) estimate as a point (as 
in Kok et al. (2015)), we will obtain samples from the 
posterior distribution, which can be used to estimate the 
MAP, mean, credibility intervals, etc. 



Fig. 4. Particle densities (on a blue-yellow-red scale, from low to 
high) for FFBSi (left) and Algorithm 3 (right). In both cases, 
50 000 particles are sampled for each t. They are, however, 
centered along the filtering distribution for the FFBSi, but 
biased toward the smoothing distribution for Algorithm 3. That 
is, Algorithm 3 explores (at least in this example) the relevant 
part of the state space (i.e., the left ‘shoulder’) better. 

5.1 Problem setup 

We take the problem as presented by Kok et al. (2015), a 
10-dimensional nonlinear non-Gaussian problem. The goal 
is to estimate the position, velocity and orientation of the 
sensor board with the UWB transmitter, accelerometer 
and gyroscope, placed on the foot of a human. The UWB 
transmitter sends out pulses at (unknown) times r^, and 
the time of arrival at the 10 receivers (indexed by m) are 
measured. The setup is calibrated using the algorithm in 
Kok et al. (2015), making sure that the receiver positions 
are known and that their clocks are synchronized. 

In the model, the state vector is xj = [pjvfqf], pt is 
the (3D) position, Vt the velocity and qt the orientation 
(parametrized using unit quaternions). The SSM is given 

+ (10a) 

f;r+i=r;r + T.ar, (10b) 

qth = qf © exp (10c) 

ym,t = Tt + - Pth + em,t (lOd) 

where (10a) - (10c) are the dynamics and (lOd) is the 
measurement equation. The superscripts n and b denote 
coordinate frames (n is the navigation frame aligned with 
gravity, and b is the body frame, aligned with the sensor 
axes of the accelerometers), c denotes the speed of light, 
0 denotes the quaternion product and exp denotes the 
vector exponential (Hoi, 2011). Tg is the time between 
two data samples from the accelerometer and gyroscope, 
sampled with 120 Hz. However, the UWB samples are 
sampled at approximately 10 Hz. Due to the nature of 
UWB measurements, em,t is modeled as 

(2 - a)A/'(0,cr^), em,t < 0, 
aGauchy(0,7), > 0, 

because measurements can only arrive later (and not 
earlier) in case of multipath and non-line-of-sight prop¬ 
agation. The acceleration a" is found via accelerometer 
measurements ya^t, modeled as 

Ua.t = ~ 5 ") + (12) 

with denoting gravity, is a rotation matrix repre¬ 
sentation of and = {Rth'^ . The angular velocity 
utt is obtained from the gyroscope measurements y^^^t as 

ybj,t = LOt + 5u} + e^i^f (13) 

The noise ea,t and e^j^t are modeled as Af{0, a^) and 
A/'(0,crJ), respectively. 6a and 6aj are sensor biases. Note 
that the accelerometer and gyroscope measurements are 





not treated as outputs in (10), but rather as inputs to 
the dynamics, implicitly introducing an uncertainty in 
f(xt+i\xt) through the measurement noise. 

5.2 Results 

Algorithm 3 was applied to data presented by Kok et al. 
(2015) with (10) - (13). The results for K = 1000 
iterations and N = 500 particles are summarized in 
Figure 5 in terms of the mean and credibility intervals (cf. 
Figure 13 and 14 in Kok et al. (2015)). For reference, the 
ground truth (obtained by an optical reference system) is 
also shown in the plot. In terms of computational load, the 
presented results took about 1 day to obtain on a standard 
desktop computer. 

Note the credibility intervals, which are the gain of using 
this method producing samples (as opposed to a method 
based on point estimates). The credibility intervals are 
varying over time and are different for different states, 
which indeed adds information to the results. 

6. CONCLUSIONS 

We have shown how the CPF-AS can be used to solve 
the nonlinear state smoothing problem in a disparate way 
compared to the currently available particle smoothers. 
The asymptotic convergence of our smoother was estab¬ 
lished, and we also illustrated the use of the smoother 
on two simulated examples and one challenging real-world 
application. 

Based on the results of Theorem 1 and the numerical ex¬ 
amples we conclude that Algorithm 3 is indeed a competi¬ 
tive alternative to the existing state-of-the-art smoothers. 
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(b) Smoothing distribution for orientation. 

Fig. 5. The mean (black line) and 99% credibility intervals (orange 
fields) of p{xi:T\yi:T) for the position and orientation states, 
and ground truth (dashed gray) from an optical reference 
system. 


The present development opens up for interesting future 
work, such as hybrid versions of FFBSi and Algorithm 3, 
where FFBSi is used to initialize Algorithm 3. Further 
studies on how to tackle the trade-off between the number 
of particles N and the number of iterations K in Algo¬ 
rithm 3 for optimal performance (given a computational 
limit) would also be interesting. 
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Abstract: This technical report gives some additional details on the numerical examples used 
in Svensson et al. (2015). 


1. INTRODUCTION 

This report contains some details on the examples pre¬ 
sented by Svensson et al. (2015). The problem under 
consideration is that of state space smoothing using the 
conditional particle hlter with ancestor sampling (CPF- 
AS). In this report, we will address additional details 
concerning 

i) the simulated nonlinear, multimodal example used in 
Section 4.2. 

ii) the indoor positioning application used in Section 5. 

Matlab code for the simulated example can be found via 
the first author’s homepage. 

2. SIMULATED NONLINEAR, MULTIMODAL 
EXAMPLE 

This section gives an extended introduction to the problem 
considered in Svensson et al. (2015, Section 4.2). The 
essential details are contained in the original article, but we 
will provide an alternative introduction. The state space 
model considered in the problem is 


Xt+l\xt " 

A/'(a:t,0.5), 

(la) 

yt\xt - 

9{yt\xt), 

(lb) 

Xi = 

= 3. 

(Ic) 


The problem concerns smoothing for a given output se¬ 
quence yi:t. Thus we write G{xt) = g{yt\xt), the likelihood 
of Xt, omitting the dependence on yt. G{xt) is defined by 
the surface of Figure 1, which is parametrized as a sum of 
six Gaussian-like functions. The specific parametrization 
is found in the provided source code. 

The state space filtering/smoothing problem arising from 
this problem is solved using three approaches. For hltering, 
the particle filter in Svensson et al. (2015, Algorithm 1), 
with qt = /, is used. For smoothing, the FFBSi with 
rejection sampling, as presented in Lindsten and Schon 
(2013, Algorithm 5), is used, together with the MCMC 
smoother in Svensson et al. (2015, Algorithm 3), and the 
results reported by Svensson et al. (2015) are obtained. 

* This work was supported by the project Probabilistic modelling of 
dynamical systems (Contract number: 621-2013-5524) funded by the 
Swedish Research Council (VR) and CADICS, a Linnaeus Center, 
funded by the Swedish Research Council (VR). 



Fig. 1. Definition of G{xt). 


3. INDOOR POSITIONING 

This section focuses on the indoor positioning applica¬ 
tion in Svensson et al. (2015, Section 5). The problem 
is discussed in Kok et al. (2015), and concerns a sensor 
fusion problem involving accelerometer, gyroscope, and 
ultrawideband (UWB) measurements. There are several 
details on the problem not covered by Svensson et al. 
(2015); the non-uniform sampling intervals of the UWB 
measurements, the unknown transmission times, the sen¬ 
sor biases, some issues on the ancestor sampling, and the 
initialization of the first conditional trajectory. 

We repeat the state space model here: 

uJVi = Ut” -b T,a^, > f{xt+i\xt, a'(,uJt) (2a) 

^r+i = 9i"'’®exp^a;t, J 

ym,t=Tt + \\r')^-pf \\2 + em,t }g{yt\xt), (2b) 

with the same notation as Svensson et al. (2015). Most 
important, x'^ = [(p")"'" (t")"’’ ('Z"^)"'"] forms the state 
vector, and a” are the acceleration, ujt the angular velocity, 
and ym,t are the UWB measurements (for receiver number 
m). The sampling frequency for the accelerometers and the 
gyroscopes is 120 Hz. 



The acceleration and angular velocity are modeled to be 
measured as 

ya,t = -Rt"(ar ~ ff”) +1^0+ Ca.i, ( 3 ) 

yu},t = Wt + (4) 

with notation as Svensson et al. (2015). Through the 
measurement noise in these models, stochastic noise enters 
the state space. 

3.1 Non-uniform sampling interval 

For every sampling step t, the accelerometer and gyro¬ 
scope data ya,t and y^^^t are available. However, the UWB 
measurements are recorded at a lower sampling rate, and 
are available only for approximately every tenth sample. 
A high level algorithm illustrating how this is handled is 
shown in Algorithm 1. 

Algorithm 1 Algorithmic strategy for the indoor posi¬ 
tioning problem 

1; for k = 1,..., K do 

2; Sample x\ from qi{xi) 

3; Dram x\ ^ p{x\),i € [1,N — V\. 

4: Set x^.rp = xi-rlk]. 

5; for t = 1,..., T — 1 do 

6: if UWB measurement {ym,t\m=i available then 

7; Set wl = g{yt\xl),i € [I,N]. 

8: Draw 5) with P (5) = j) oc w(_i,i G [1, A — 1]. 

9: Set x\,t = {x\lt_j^,xi},ie [1, A - 1]. 

10; end if 

11; Draw xj+i from/(xj+i lx), at, wt),! e [1,A-1]. 

12; Draw with P = j) oc f{xf^^\xi). 

13; Setx)^t+i = {^i!rA^+i}- 

14; end for 

15; Draw J with P(z = J) oc and set xi.,T[k -|- 1] = 

^l:T- 

16; end for 


(Note that the notation for the ancestor sampling variable is b, 
instead of a, not to confuse it with the accelerometer data.) 

3.2 Unknown transmission times 

When evaluating (2b), the transmission time Tt is un¬ 
known. It is approximately handled by Monte Carlo in¬ 
tegration: 

9{yt\xt) =Pe{ym,t -Tt - \\rl^-Pth I Tt) 

-hl-p7h), (5) 

3 

where Pe is the pdf defined by eq. (11) in Svensson et al. 
(2015), and rf are samples with possible values of r. All 
particles (indexed with i) are evaluated with the same set 
of samples r/. 

3.3 Sensor bias 

The numerical values of the sensor biases da and are 
small, compared to the noise levels. They were therefore 
approximated manually. However, a more thorough system 
identification approach can be applied within the CPF-AS 
framework, as proposed by Lindsten (2013). 


3.4 Evaluation of f{xt+i\xt,af ,ujt) 

For the ancestor sampling in Step 1 in Algorithm 1, 
f{xt+i\xt,af,ujt) needs to be evaluated. This involves the 
quaternion product and vector exponential in (2a), which 
can be manipulated as follows: 

Qt+i = qf © exp ^(wt -b <t4> 

e.,t = ^log((gf)-'©<zr+i)-^z (6) 

where log denotes the logarithm for unit quaternions (Hoi, 
2011). An approximation of this expression was used in the 
computations. 

3.5 Low chance of ‘new ancestor’ 

One challenge is the small uncertainty in the position (2a), 

because of the (physically reasonable) factor if- (« 10“°) 
in front of the noise term. To handle this, the uncertainty 
was artificially increased in the ancestor sampling step. 
This causes a substantial increase in the mixing, but also 
a non-feasible ‘jump’ in the smoothing trajectories. These 
artificial ‘jumps’ appears, however, to be rare and can also 
be expected to ‘even out’ as A —> 00 , but might indeed 
cause an overestimate of the variance. 

A more thorough treatment would be to apply the recent 
development by Lindsten et al. (2015). 

3.6 Initialization 

The model has a state space of 9 dimensions (parametrized 
using 10 variables). The structure of the model and the un¬ 
certainties cause, according to our experience, the particle 
filter to diverge quickly if A is not sufficiently large. To 
speed up the initialization phase, a more ‘loose’ model was 
used in the first run of the CPF-AS to find one reasonable 
trajectory. This non-diverging trajectory was then used as 
the conditional trajectory for the subsequent run with the 
correct model. 
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